﻿#pragma once

#include <array>
#include <cmath>
#include <memory>
#include <type_traits>
#include <utility>

inline static constexpr float PI = 3.14159265358979f;
inline static constexpr float DEG_RAD2 = PI / 360.0f;
inline static constexpr float DEG_RAD = 180.f / PI;
inline static constexpr float RAD_DEG = PI / 180.f;

namespace ImmeMath {

template <typename _Ty>
inline constexpr _Ty  lerp_imme(_Ty a, _Ty b, _Ty t)
	requires(std::is_arithmetic_v<_Ty>)
{
	return a + t * (b - a);
}

template <typename _Ty>
inline constexpr _Ty  mod_imme(_Ty a, _Ty b)
	requires(std::is_arithmetic_v<_Ty>)
{
	if consteval {
		_Ty discuss = a / b;
		if (discuss <= (_Ty)INT64_MAX && discuss >= (_Ty)INT64_MIN)
			discuss = (_Ty)((int64_t)discuss);
		return a - b * discuss;

	} else {
		return ::fmod(a, b);
	}
}

template <typename _Ty>
inline constexpr _Ty  abs_imme(_Ty is)
	requires(std::is_arithmetic_v<_Ty>)
{
	if consteval {
		return is > (_Ty)0 ? is : -is;
	} else {
		return ::abs(is);
	}
}

template <typename _Ty>
inline constexpr _Ty  floor_imme(_Ty x)
	requires(std::is_arithmetic_v<_Ty>)
{
	if consteval {
		if constexpr (std::integral<_Ty>)
			return x;

		if (x > (_Ty)INT64_MAX || x < (_Ty)INT64_MIN)
			return x;
		else if (x > (_Ty)0)
			return (_Ty)((int64_t)x);
		else
			return (_Ty)((int64_t)(x - (_Ty)1));
	} else {
		return ::floor(x);
	}
}

template <typename _Ty>
inline constexpr _Ty  ceil_imme(_Ty x)
	requires(std::is_arithmetic_v<_Ty>)
{
	if consteval {
		if constexpr (std::integral<_Ty>)
			return x;

		if (x > (_Ty)INT64_MAX || x < (_Ty)INT64_MIN)
			return x;
		else if (x > (_Ty)0)
			return (_Ty)((int64_t)(x + (_Ty)1));
		else
			return (_Ty)((int64_t)x);
	} else {
		return ::ceil(x);
	}
}

constexpr float sqrtf_imme(float x) {
	if (x < 0.f)
		return ::sqrtf(x);
	else {
		if consteval {
			if (x == 0.f)
				return 0.f;  //
			constexpr float err = 1e-6f;
			float res = x / 2.f;
			for (int32_t i = 0; i < 30 && abs_imme(res - x / res) > err * res; ++i)
				res = (x / res + res) / 2.f;
			return res;
		} else {
			return ::sqrtf(x);
		}
	}
}

inline constexpr float tangent[] = {
	1.f,
	1 / 2.f,
	1 / 4.f,
	1 / 8.f,
	1 / 16.f,
	1 / 32.f,
	1 / 64.f,
	1 / 128.f,
	1 / 256.f,
	1 / 512.f,
	1 / 1024.f,
	1 / 2048.f,
	1 / 4096.f,
	1 / 8192.f,
	1 / 16384.f,
	1 / 32768.f,
	1 / 65536.f,
	1 / 131072.f,
	1 / 262144.f,
	1 / 524288.f,
	1 / 1048576.f,
	1 / 2097152.f,
	1 / 4194304.f,
	1 / 8388608.f,
	1 / 16777216.f,
	1 / 33554432.f,
	1 / 67108864.f,
	1 / 134217728.f,
	1 / 268435456.f,
	1 / 536870912.f,
	1 / 1073741824.f,
	1 / 2147483648.f,
	1 / 4294967296.f,
	1 / 8589934592.f,
	1 / 17179869184.f,
	1 / 34359738368.f,
};

inline constexpr float angle[] = {
	45.0f,
	26.565051177078f,
	14.0362434679265f,
	7.1250163489018f,
	3.57633437499735f,
	1.78991060824607f,
	0.8951737102111f,
	0.4476141708606f,
	0.2238105003685f,
	0.1119056770662f,
	0.0559528918938f,
	0.027976452617f,
	0.01398822714227f,
	0.006994113675353f,
	0.003497056850704f,
	1.748528427e-3f,
	8.742642137e-4f,
	4.371321069e-4f,
	2.185660534e-4f,
	1.092830267e-4f,
	5.464151336e-5f,
	2.732075668e-5f,
	1.366037834e-5f,
	6.83018917e-6f,
	3.415094585e-6f,
	1.707547293e-6f,
	8.537736463e-7f,
	4.268868231e-7f,
	2.134434116e-7f,
	1.067217058e-7f,
	5.336085289e-8f,
	2.668042645e-8f,
	1.334021322e-8f,
	6.670106611e-9f,
	3.335053306e-9f,
	1.667526653e-9f,
};

constexpr std::pair<float, float> sincosdf_imme(float a) {
	if consteval {
		while (a > 360.f)
			a -= 360.f;
		while (a < 0.f)
			a += 360.f;

		bool sinminus = false;
		bool cosminus = false;

		if (a <= 90.f) {
		} else if (a <= 180.f) {
			cosminus = true;
			a = 180.f - a;
		} else if (a <= 270.f) {
			sinminus = true;
			cosminus = true;
			a = a - 180.f;
		} else {
			sinminus = true;
			a = (360.f - a);
		}

		float x = 1.f;
		float y = 0.f;
		constexpr float k = 0.6072528827f;
		float x_new = 0.f;
		float y_new = 0.f;

		for (int32_t i = 0; i < 30; ++i) {
			if (a > 0.f) {
				x_new = (x - y * tangent[i]);
				y_new = (y + x * tangent[i]);
				x = x_new;
				y = y_new;
				a -= angle[i];
			} else {
				x_new = (x + y * tangent[i]);
				y_new = (y - x * tangent[i]);
				x = x_new;
				y = y_new;
				a += angle[i];
			}
		}
		return std::make_pair<float, float>(y * (sinminus ? -k : k), x * (cosminus ? -k : k));
	} else {
		return std::make_pair<float, float>(::sinf(a / DEG_RAD), ::cosf(a / DEG_RAD));
	}
}

constexpr float sindf_imme(float x) {
	if consteval {
		auto i = sincosdf_imme(x);
		return std::get<0>(i);
	} else {
		return ::sinf(x / DEG_RAD);
	}
}

constexpr float cosdf_imme(float x) {
	if consteval {
		auto i = sincosdf_imme(x);
		return std::get<1>(i);
	} else {
		return ::cosf(x / DEG_RAD);
	}
}

constexpr float tandf_imme(float x) {
	if consteval {
		auto i = sincosdf_imme(x);
		return std::get<0>(i) / std::get<1>(i);
	} else {
		return ::tanf(x / DEG_RAD);
	}
}

constexpr float atandf_imme(float x) {
	if consteval {
		if (x == 0.f)
			return 0.f;

		bool minus = false;
		if (x < 0.f) {
			minus = true;
			x = -x;
		}

		float y = 1.f;
		x = 1.f / x;

		float x_new = 0.f;
		float y_new = 0.f;
		float angleSum = 0.f;

		for (int32_t i = 0; i < 30; ++i) {
			if (y > 0.f) {
				x_new = x + y * tangent[i];
				y_new = y - x * tangent[i];
				x = x_new;
				y = y_new;
				angleSum += angle[i];
			} else {
				x_new = x - y * tangent[i];
				y_new = y + x * tangent[i];
				x = x_new;
				y = y_new;
				angleSum -= angle[i];
			}
		}
		return minus ? -angleSum : angleSum;
	} else {
		return ::atanf(x) * DEG_RAD;
	}
}

constexpr float atan2df_imme(float y, float x) {
	if consteval {
		if (x == 0.f && y == 0.f)
			return 0.f;  // undefined
		else if (x > 0.f)
			return atandf_imme(y / x);
		else if (y >= 0.f && x < 0.f)
			return atandf_imme(y / x) + 180.f;
		else if (y < 0.f && x < 0.f)
			return atandf_imme(y / x) - 180.f;
		else if (y > 0.f && x == 0.f)
			return 90.f;
		else if (y < 0.f && x == 0.f)
			return -90.f;
		return 0.f;  // no meaning
	} else {
		return ::atan2f(y, x) * DEG_RAD;
	}
}

constexpr float asindf_imme(float x) {
	if (ImmeMath::abs_imme(x) > 1.f)
		return ::asinf(x) * DEG_RAD;
	else {
		if consteval {
			return atandf_imme(x / sqrtf_imme(1.f - x * x));
		} else {
			return ::asinf(x) * DEG_RAD;
		}
	}
}

constexpr float acosdf_imme(float x) {
	if (ImmeMath::abs_imme(x) > 1.f)
		return ::acosf(x) * DEG_RAD;
	else {
		if consteval {
			return atandf_imme(sqrtf_imme(1.f - x * x) / x);
		} else {
			return ::acosf(x) * DEG_RAD;
		}
	}
}

}  // namespace ImmeMath

struct vec2_t {
	float x{};
	float y{};
};

inline constexpr vec2_t fromAngle(float angle) noexcept {
	return vec2_t{-ImmeMath::sindf_imme(angle), ImmeMath::cosdf_imme(angle)};
}

__forceinline constexpr bool operator==(const vec2_t a, const vec2_t o) noexcept {
	return a.x == o.x && a.y == o.y;
}

inline constexpr vec2_t add(vec2_t a, const vec2_t o) noexcept {
	a.x += o.x;
	a.y += o.y;
	return a;
}

inline constexpr vec2_t operator+(const vec2_t a, const vec2_t o) noexcept {
	return add(a, o);
}

inline constexpr vec2_t add(vec2_t a, const float o) noexcept {
	a.x += o;
	a.y += o;
	return a;
}
inline constexpr vec2_t operator+(const vec2_t a, const float o) noexcept {
	return add(a, o);
}

inline constexpr vec2_t add(vec2_t a, const float ox, const float oy) noexcept {
	a.x += ox;
	a.y += oy;
	return a;
}

inline constexpr vec2_t operator-(vec2_t a) noexcept {
	a.x = -a.x;
	a.y = -a.y;
	return a;
}

inline constexpr vec2_t sub(vec2_t a, const float f) noexcept {
	a.x -= f;
	a.y -= f;
	return a;
}

inline constexpr vec2_t operator-(const vec2_t a, const float f) noexcept {
	return sub(a, f);
}

inline constexpr vec2_t sub(vec2_t a, const float ox, const float oy) noexcept {
	a.x -= ox;
	a.y -= oy;
	return a;
}

inline constexpr vec2_t sub(vec2_t a, const vec2_t o) noexcept {
	a.x -= o.x;
	a.y -= o.y;
	return a;
}

inline constexpr vec2_t operator-(const vec2_t a, const vec2_t o) noexcept {
	return sub(a, o);
}

inline constexpr vec2_t mul(vec2_t a, const vec2_t o) noexcept {
	a.x *= o.x;
	a.y *= o.y;
	return a;
}

inline constexpr vec2_t operator*(const vec2_t a, const vec2_t o) noexcept {
	return mul(a, o);
}

inline constexpr vec2_t mul(vec2_t a, const float f) noexcept {
	a.x *= f;
	a.y *= f;
	return a;
}

inline constexpr vec2_t operator*(const vec2_t a, const float f) noexcept {
	return mul(a, f);
}

inline constexpr vec2_t div(vec2_t a, const float f) noexcept {
	a.x /= f;
	a.y /= f;
	return a;
}

inline constexpr vec2_t div(vec2_t a, const vec2_t o) noexcept {
	a.x /= o.x;
	a.y /= o.y;
	return a;
}

__forceinline constexpr float squaredlen(const vec2_t a) noexcept {
	return a.x * a.x + a.y * a.y;
}

inline constexpr float magnitude(const vec2_t a) noexcept {
	return ImmeMath::sqrtf_imme(squaredlen(a));
}

inline constexpr vec2_t normalized(const vec2_t a) noexcept {
	return div(a, magnitude(a));
}

inline constexpr vec2_t cross(vec2_t a) noexcept {
	const float temp = a.x;
	a.x = -a.y;
	a.y = temp;
	return a;
}

__forceinline constexpr float dot(const vec2_t a, float ox, float oy) noexcept {
	return a.x * ox + a.y * oy;
}

__forceinline constexpr float dot(const vec2_t a, const vec2_t o) noexcept {
	return a.x * o.x + a.y * o.y;
}

inline constexpr vec2_t normAngles(vec2_t a) noexcept {
	while (a.x > 90.f)
		a.x -= 180.0f;
	while (a.x < -90.f)
		a.x += 180.0f;

	while (a.y > 180.0f)
		a.y -= 360.0f;
	while (a.y < -180.0f)
		a.y += 360.0f;
	return a;
}

struct vec3_t {
	float x{};
	float y{};
	float z{};
};

__forceinline constexpr bool iszero(const vec3_t a) noexcept {
	return a.x == 0 && a.y == 0 && a.z == 0;
}

__forceinline constexpr bool operator==(const vec3_t a, const vec3_t o) noexcept {
	return a.x == o.x && a.y == o.y && a.z == o.z;
};

inline constexpr vec3_t add(vec3_t a, const float f) noexcept {
	a.x += f;
	a.y += f;
	a.z += f;
	return a;
};

inline constexpr vec3_t operator+(const vec3_t a, const float f) noexcept {
	return add(a, f);
}

inline constexpr vec3_t add(vec3_t a, const float x1, const float y1, const float z1) noexcept {
	a.x += x1;
	a.y += y1;
	a.z += z1;
	return a;
};

inline constexpr vec3_t add(vec3_t a, const vec3_t o) noexcept {
	a.x += o.x;
	a.y += o.y;
	a.z += o.z;
	return a;
}

inline constexpr vec3_t operator+(const vec3_t a, const vec3_t o) noexcept {
	return add(a, o);
}

inline constexpr vec3_t operator-(vec3_t a) noexcept {
	a.x = -a.x;
	a.y = -a.y;
	a.z = -a.z;
	return a;
};

inline constexpr vec3_t sub(vec3_t a, const float f) noexcept {
	a.x -= f;
	a.y -= f;
	a.z -= f;
	return a;
};

inline constexpr vec3_t operator-(const vec3_t a, const float f) noexcept {
	return sub(a, f);
}

inline constexpr vec3_t sub(vec3_t a, const float x1, const float y1, const float z1) noexcept {
	a.x -= x1;
	a.y -= y1;
	a.z -= z1;
	return a;
};

inline constexpr vec3_t sub(vec3_t a, const vec3_t o) noexcept {
	a.x -= o.x;
	a.y -= o.y;
	a.z -= o.z;
	return a;
}

inline constexpr vec3_t operator-(const vec3_t a, const vec3_t o) noexcept {
	return sub(a, o);
}

inline constexpr vec3_t mul(vec3_t a, const vec3_t o) noexcept {
	a.x *= o.x;
	a.y *= o.y;
	a.z *= o.z;
	return a;
};

inline constexpr vec3_t operator*(const vec3_t a, const vec3_t o) noexcept {
	return mul(a, o);
}

inline constexpr vec3_t mul(vec3_t a, const float f) noexcept {
	a.x *= f;
	a.y *= f;
	a.z *= f;
	return a;
};

inline constexpr vec3_t operator*(const vec3_t a, const float f) noexcept {
	return mul(a, f);
}

inline constexpr vec3_t mul(vec3_t a, const float x1, const float y1, const float z1) noexcept {
	a.x *= x1;
	a.y *= y1;
	a.z *= z1;
	return a;
};

inline constexpr vec3_t div(vec3_t a, const float f) noexcept {
	a.x /= f;
	a.y /= f;
	a.z /= f;
	return a;
};

inline constexpr vec3_t operator/(const vec3_t a, const float f) noexcept {
	return div(a, f);
}

inline constexpr vec3_t div(vec3_t a, const float x1, const float y1, const float z1) noexcept {
	a.x /= x1;
	a.y /= y1;
	a.z /= z1;
	return a;
};

inline constexpr vec3_t div(vec3_t a, const vec3_t o) noexcept {
	a.x /= o.x;
	a.y /= o.y;
	a.z /= o.z;
	return a;
};

inline constexpr vec3_t operator/(const vec3_t a, const vec3_t o) noexcept {
	return div(a, o);
}

inline constexpr vec3_t floor(vec3_t a) noexcept {
	a.x = ImmeMath::floor_imme(a.x);
	a.y = ImmeMath::floor_imme(a.y);
	a.z = ImmeMath::floor_imme(a.z);
	return a;
};

__forceinline constexpr float squaredlen(const vec3_t a) noexcept {
	return a.x * a.x + a.y * a.y + a.z * a.z;
}

__forceinline constexpr float squaredxzlen(const vec3_t a) noexcept {
	return a.x * a.x + a.z * a.z;
}

inline constexpr vec3_t lerp(vec3_t a, const vec3_t other, const float tx, const float ty, const float tz) noexcept {
	a.x += tx * (other.x - a.x);
	a.y += ty * (other.y - a.y);
	a.z += tz * (other.z - a.z);
	return a;
}

inline constexpr vec3_t lerp(vec3_t a, const vec3_t other, const float val) noexcept {
	a.x += val * (other.x - a.x);
	a.y += val * (other.y - a.y);
	a.z += val * (other.z - a.z);
	return a;
}

inline constexpr vec2_t flatten(const vec3_t a) noexcept {
	return vec2_t{a.x, a.y};
}
//...
inline constexpr float sqrxy(const vec3_t a) noexcept {
	return a.x * a.x + a.y * a.y;
}

inline constexpr float dot(const vec3_t a, const vec3_t o) noexcept {
	return a.x * o.x + a.y * o.y + a.z * o.z;
}

inline constexpr float dotxy(const vec3_t a, const vec3_t o) noexcept {
	return a.x * o.x + a.y * o.y;
}

inline constexpr float magnitude(const vec3_t a) noexcept {
	return ImmeMath::sqrtf_imme(squaredlen(a));
}

inline constexpr vec3_t normalize(const vec3_t a) noexcept {
	return div(a, magnitude(a));
}

inline constexpr float dist(const vec3_t a, const vec3_t e) noexcept {
	return magnitude(sub(a, e));
}

inline constexpr float GetXYDist(const vec3_t a, const vec3_t e) noexcept {
	const float dx = e.x - a.x;
	const float dy = e.y - a.y;
	return ImmeMath::sqrtf_imme(dx * dx + dy * dy);
}

inline constexpr float GetXZDist(const vec3_t a, const vec3_t e) noexcept {
	const float dx = e.x - a.x;
	const float dz = e.z - a.z;
	return ImmeMath::sqrtf_imme(dx * dx + dz * dz);
}

inline constexpr float magnitudexy(const vec3_t a) noexcept {
	return ImmeMath::sqrtf_imme(a.x * a.x + a.y * a.y);
}

inline constexpr float magnitudexz(const vec3_t a) noexcept {
	return ImmeMath::sqrtf_imme(a.x * a.x + a.z * a.z);
}

inline constexpr vec3_t cross(const vec3_t a, const vec3_t b) noexcept {
	return vec3_t{a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
}

inline constexpr float cxy(const vec3_t a, const vec3_t b) noexcept {
	return a.x * b.y - a.y * b.x;
}

inline constexpr vec2_t CalcAngle(const vec3_t a, vec3_t dst) noexcept {  // this
	vec3_t diff = sub(dst, a);
	if (diff.x == 0.f && diff.y == 0.f && diff.z == 0.f)
		return vec2_t{0.f, 0.f};
	diff.y /= magnitude(diff);
	return vec2_t{-ImmeMath::asindf_imme(diff.y), -ImmeMath::atan2df_imme(diff.x, diff.z)};
}

struct vec3_ti {
	int32_t x{};
	int32_t y{};
	int32_t z{};
};

__forceinline constexpr bool iszero(const vec3_ti a) noexcept {
	return a.x == 0 && a.y == 0 && a.z == 0;
}

__forceinline constexpr bool operator==(const vec3_ti a, const vec3_ti& o) noexcept {
	return a.x == o.x && a.y == o.y && a.z == o.z;
}

inline constexpr vec3_ti add(vec3_ti a, const vec3_ti o) noexcept {
	a.x += o.x;
	a.y += o.y;
	a.z += o.z;
	return a;
}

inline constexpr vec3_ti operator+(const vec3_ti a, const vec3_ti o) noexcept {
	return add(a, o);
}

inline constexpr vec3_ti add(vec3_ti a, const int32_t f) noexcept {
	a.x += f;
	a.y += f;
	a.z += f;
	return a;
}

inline constexpr vec3_ti operator+(const vec3_ti a, const int32_t f) noexcept {
	return add(a, f);
}

inline constexpr vec3_ti add(vec3_ti a, const int32_t ox, const int32_t oy, const int32_t oz) noexcept {
	a.x += ox;
	a.y += oy;
	a.z += oz;
	return a;
};

inline constexpr vec3_ti sub(vec3_ti a, const int32_t ox, const int32_t oy, const int32_t oz) noexcept {
	a.x -= ox;
	a.y -= oy;
	a.z -= oz;
	return a;
}

inline constexpr vec3_ti sub(vec3_ti a, const vec3_ti o) noexcept {
	a.x -= o.x;
	a.y -= o.y;
	a.z -= o.z;
	return a;
}

inline constexpr vec3_ti operator-(const vec3_ti a, const vec3_ti o) noexcept {
	return sub(a, o);
}

inline constexpr vec3_t toVec3t(const vec3_ti a) noexcept {
	return vec3_t{static_cast<float>(a.x), static_cast<float>(a.y), static_cast<float>(a.z)};
}

inline constexpr vec3_ti toVec3ti(const vec3_t a) noexcept {
	return vec3_ti{static_cast<int32_t>(a.x), static_cast<int32_t>(a.y), static_cast<int32_t>(a.z)};
}

struct vec4_t {
	float x{};
	float y{};
	float z{};
	float w{};

	inline constexpr float& operator[](uint8_t i) {
		[[assume(i >= 0 && i < 4)]];
		switch (i) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
		case 3:
			return w;
		default:
			std::unreachable();
		}
	};
};

inline constexpr bool contains(const vec4_t a, const vec2_t point) noexcept {
	if (point.x <= a.x || point.y <= a.y)
		return false;

	if (point.x >= a.z || point.y >= a.w)
		return false;
	return true;
};

struct glmatrixf {
public:
	float v[16]{};

public:
	__forceinline constexpr glmatrixf() = default;
	__forceinline constexpr float operator[](uint8_t i) const { return v[i]; }
	__forceinline constexpr float& operator[](uint8_t i) { return v[i]; }

#define MULMAT(row, col) v[col + row] = x[row] * y[col] + x[row + 4] * y[col + 1] + x[row + 8] * y[col + 2] + x[row + 12] * y[col + 3];

	template <class XT, class YT>
	inline constexpr void mul(const XT x[16], const YT y[16]) {
		MULMAT(0, 0);
		MULMAT(1, 0);
		MULMAT(2, 0);
		MULMAT(3, 0);
		MULMAT(0, 4);
		MULMAT(1, 4);
		MULMAT(2, 4);
		MULMAT(3, 4);
		MULMAT(0, 8);
		MULMAT(1, 8);
		MULMAT(2, 8);
		MULMAT(3, 8);
		MULMAT(0, 12);
		MULMAT(1, 12);
		MULMAT(2, 12);
		MULMAT(3, 12);
	}

#undef MULMAT

	constexpr glmatrixf correct() const {
		glmatrixf newMatPtr{};

		for (int32_t i = 0; i < 4; ++i) {
			newMatPtr.v[i * 4 + 0] = v[0 + i];
			newMatPtr.v[i * 4 + 1] = v[4 + i];
			newMatPtr.v[i * 4 + 2] = v[8 + i];
			newMatPtr.v[i * 4 + 3] = v[12 + i];
		}
		return newMatPtr;
	};

	constexpr bool OWorldToScreen(vec3_t origin, vec3_t pos, vec2_t& screen, vec2_t fov, vec2_t displaySize) const {
		pos = sub(pos, origin);

		float x = transformx(pos);
		float y = transformy(pos);
		float z = transformz(pos);

		if (z > 0)
			return false;

		float mX = (float)displaySize.x / 2.0F;
		float mY = (float)displaySize.y / 2.0F;

		screen.x = mX + (mX * x / -z * fov.x);
		screen.y = mY - (mY * y / -z * fov.y);

		return true;
	}

	inline constexpr void mul(const glmatrixf& x, const glmatrixf& y) {
		mul(x.v, y.v);
	}

	inline constexpr void translate(float x, float y, float z) {
		v[12] += x;
		v[13] += y;
		v[14] += z;
	}

	inline constexpr void translate(const vec3_t o) {
		translate(o.x, o.y, o.z);
	}

	inline constexpr void scale(float x, float y, float z) {
		v[0] *= x;
		v[1] *= x;
		v[2] *= x;
		v[3] *= x;
		v[4] *= y;
		v[5] *= y;
		v[6] *= y;
		v[7] *= y;
		v[8] *= z;
		v[9] *= z;
		v[10] *= z;
		v[11] *= z;
	}

	inline constexpr void invertnormal(vec3_t& dir) const {
		vec3_t n(dir);
		dir.x = n.x * v[0] + n.y * v[1] + n.z * v[2];
		dir.y = n.x * v[4] + n.y * v[5] + n.z * v[6];
		dir.z = n.x * v[8] + n.y * v[9] + n.z * v[10];
	}

	inline constexpr void invertvertex(vec3_t& pos) const {
		vec3_t p(pos);
		p.x -= v[12];
		p.y -= v[13];
		p.z -= v[14];
		pos.x = p.x * v[0] + p.y * v[1] + p.z * v[2];
		pos.y = p.x * v[4] + p.y * v[5] + p.z * v[6];
		pos.z = p.x * v[8] + p.y * v[9] + p.z * v[10];
	}

	inline constexpr void transform(const vec3_t in, vec4_t& out) const {
		out.x = transformx(in);
		out.y = transformy(in);
		out.z = transformz(in);
		out.w = transformw(in);
	}

	__forceinline constexpr float transformx(const vec3_t p) const {
		return p.x * v[0] + p.y * v[4] + p.z * v[8] + v[12];
	}

	__forceinline constexpr float transformy(const vec3_t p) const {
		return p.x * v[1] + p.y * v[5] + p.z * v[9] + v[13];
	}

	__forceinline constexpr float transformz(const vec3_t p) const {
		return p.x * v[2] + p.y * v[6] + p.z * v[10] + v[14];
	}

	__forceinline constexpr float transformw(const vec3_t p) const {
		return p.x * v[3] + p.y * v[7] + p.z * v[11] + v[15];
	}

	__forceinline constexpr vec3_t gettranslation() const {
		return vec3_t{v[12], v[13], v[14]};
	}

	// assault cube world2screen
	constexpr vec3_t transform(glmatrixf* matrix, vec3_t& totransform) const {
		return div(vec3_t{matrix->transformx(totransform),
						  matrix->transformy(totransform),
						  matrix->transformz(totransform)},
				   (matrix->transformw(totransform)));
	}

	/// pos should be the exact center of the enemy model for scaling to work properly
	constexpr vec3_t WorldToScreen(vec3_t pos, int32_t width, int32_t height) const {
		// Matrix-vector Product, multiplying world(eye) coordinates by projection matrix = clipCoords
		float clipCoordsx = pos.x * v[0] + pos.y * v[4] + pos.z * v[8] + v[12];
		float clipCoordsy = pos.x * v[1] + pos.y * v[5] + pos.z * v[9] + v[13];
		float clipCoordsw = pos.x * v[3] + pos.y * v[7] + pos.z * v[11] + v[15];

		// perspective division, dividing by clip.W = Normalized Device Coordinates
		float NDCx = clipCoordsx / clipCoordsw;
		float NDCy = clipCoordsy / clipCoordsw;

		// viewport tranform to screenCooords

		vec3_t playerscreen{};
		playerscreen.x = (float(width / 2) * NDCx) + (NDCx + float(width / 2));
		playerscreen.y = -(float(height / 2) * NDCy) + (NDCy + float(height / 2));

		return playerscreen;
	}
};

struct AABB {
	vec3_t lower{0.f, 0.f, 0.f};
	vec3_t upper{0.f, 0.f, 0.f};
};

inline constexpr bool operator==(const AABB a, const AABB rhs) noexcept {
	return a.lower == rhs.lower && a.upper == rhs.upper;
}

inline constexpr bool isFullBlock(const AABB a) noexcept {
	const auto diff = sub(a.lower, a.upper);
	return ImmeMath::abs_imme(diff.y) == 1.f && ImmeMath::abs_imme(diff.x) == 1.f && ImmeMath::abs_imme(diff.z) == 1.f;
}

inline constexpr AABB expanded(const AABB a, const float amount) noexcept {
	return AABB{sub(a.lower, amount), add(a.upper, amount)};
}

inline constexpr AABB expandedXZ(const AABB a, float amount) noexcept {
	return AABB{sub(a.lower, amount, 0.f, amount), add(a.upper, amount, 0.f, amount)};
}

inline constexpr vec3_t centerPoint(const AABB a) noexcept {
	return add(a.lower, mul(sub(a.upper, a.lower), 0.5f));
}

inline constexpr bool intersects(const AABB a, const AABB aabb) noexcept {
	return aabb.upper.x > a.lower.x && a.upper.x > aabb.lower.x &&
		   aabb.upper.y > a.lower.y && a.upper.y > aabb.lower.y &&
		   aabb.upper.z > a.lower.z && a.upper.z > aabb.lower.z;
}

inline constexpr bool intersectsXZ(const AABB a, const AABB aabb) noexcept {
	return aabb.upper.x > a.lower.x && a.upper.x > aabb.lower.x &&
		   aabb.upper.z > a.lower.z && a.upper.z > aabb.lower.z;
}

struct MC_Color {
	float r{};
	float g{};
	float b{};
	float a{};

	constexpr MC_Color lerp(const MC_Color o, float t) const {
		return MC_Color{ImmeMath::lerp_imme(r, o.r, t), ImmeMath::lerp_imme(g, o.g, t), ImmeMath::lerp_imme(b, o.b, t), ImmeMath::lerp_imme(a, o.a, t)};
	}
};